#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "datadealer.h"

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    step1 = true;
    connect(this, SIGNAL(readyDealImage(QStringList*)), this, SLOT(deal_image_data(QStringList*)));
    connect(this, SIGNAL(readyDealPointCloud(QStringList*)), this, SLOT(deal_pointCloud_data(QStringList*)));
    connect(this, SIGNAL(readyDealPose(QStringList*)), this, SLOT(deal_pose_data(QStringList*)));
}

MainWindow::~MainWindow()
{
    delete ui;
    if(bag.isOpen())
        bag.close();
}


void MainWindow::on_choose_bag_btn_clicked()
{
    bagfileName = QFileDialog::getOpenFileName(this, "Choose ROS bag", "..", "ROS bag(*.bag)");
    ui->bag_file_label->setEnabled(true);
    ui->bag_file_label->setText(bagfileName);
    ui->bag_file_label->setFont(QFont("Sans Serif", 10, QFont::Bold));
    ui->tabWidget->setCurrentIndex(0);

    rosbag::Bag bag;
    bag.open(bagfileName.toStdString(), rosbag::bagmode::Read);
    imageTopics.empty();
    pointCloudTopics.empty();
    poseTopics.empty();
    dummyTopics.empty();
    rosbag::View view(bag);
    ui->informationText->setPlainText("=======\n");
    ros::Time bag_begin_time = view.getBeginTime();
    ros::Time bag_end_time = view.getEndTime();
    ui->informationText->appendPlainText(QString("Bag duration: %1 seconds").
                                         arg((bag_end_time-bag_begin_time).toSec()));
    auto connections = view.getConnections();
    BOOST_FOREACH (const rosbag::ConnectionInfo* connection, connections) {
        QString datatype = QString::fromStdString(connection->datatype);
        QString topic = QString::fromStdString(connection->topic);
        if(topic == "/tf_static")
            continue;
        if(datatype == "sensor_msgs/Image")
            imageTopics << topic;
        else if(datatype == "sensor_msgs/PointCloud2")
            pointCloudTopics << topic;
        else if(datatype.contains("pose", Qt::CaseInsensitive))
            poseTopics << topic;
        else if(datatype.contains("tf", Qt::CaseInsensitive))
            poseTopics << topic;
        else
            dummyTopics << topic;
    }
    ui->informationText->appendPlainText("======\nImage topics\n");
    for(auto it = imageTopics.cbegin(); it != imageTopics.cend(); it++)
        ui->informationText->appendPlainText(*it);
    ui->informationText->appendPlainText("======\nPointCloud topics\n");
    for(auto it = pointCloudTopics.cbegin(); it != pointCloudTopics.cend(); it++)
        ui->informationText->appendPlainText(*it);
    ui->informationText->appendPlainText("======\nPose topics\n");
    for(auto it = poseTopics.cbegin(); it != poseTopics.cend(); it++)
        ui->informationText->appendPlainText(*it);
    setTopicsSelector();
    ui->next_btn->setEnabled(true);
}



void MainWindow::on_actionOpen_triggered()
{
    this->on_choose_bag_btn_clicked();
}


void MainWindow::on_actionClose_triggered()
{
    this->close();
}

void MainWindow::on_next_btn_clicked()
{
    if(step1)
    {
        if(imageTopics.isEmpty() && pointCloudTopics.isEmpty() && poseTopics.isEmpty())
        {
            QMessageBox::warning(this, "No bag data!",
                                 "You should open and parse a valid ROS bag",
                                 QMessageBox::Ok);
        }
        else if(treeWidgetHasChecked() == true)
        {
            checkedTopics = getCheckedTopics();
            int cnt = 0;
            for(auto it = checkedTopics.cbegin(); it != checkedTopics.cend(); it++)
            {
                ui->next_btn->setEnabled(false);
                if((*it)->isEmpty() == false)
                {
                    switch (cnt) {
                    case 0:
                        emit readyDealImage(*it);
                        break;
                    case 1:
                        emit readyDealPointCloud(*it);
                        break;
                    case 2:
                        emit readyDealPose(*it);
                        break;
                    default:
                        QMessageBox::critical(this,
                                              "weird error",
                                              "some weird error happened",
                                              QMessageBox::Ok,
                                              QMessageBox::Cancel);
                    }
                }
                ++cnt;
            }
        }
        else
        {
            QMessageBox::warning(this,
                                 "No topic selected",
                                 "Please select at least 1 topic for generation",
                                 QMessageBox::Ok);
        }
        step1 = false;
        ui->back_btn->setEnabled(true);
    }
    else
    {
        tfVec.empty();
        ui->back_btn->setEnabled(false);
        int row = 0;
        int checkedCnt = 0;
        int selectedRow = -1;
        while(row < tfWidget->count())
        {
            if(tfWidget->item(row)->checkState() == Qt::Checked)
            {
                ++checkedCnt;
                selectedRow = row;
            }
            ++row;
        }
        if(checkedCnt != 1)
        {
            QMessageBox::warning(this,
                                 "Invalid number of tf checked",
                                 "You must choose only 1 transformation",
                                 QMessageBox::Ok);
        }
        else
        {
            if(targetDir.isEmpty())
                on_select_tardir_btn_clicked();
            qDebug() << targetDir;
            qDebug() << tfVec.at(selectedRow).first << "---->" << tfVec.at(selectedRow).second;
            PoseDataDealer *posedealer = DataDealerFactory<PoseDataDealer>::getInstance()->createDataDealer();
            posedealer->process(bagfileName, targetDir,tfVec.at(selectedRow).first, tfVec.at(selectedRow).second);

        }
    }
}

void MainWindow::deal_image_data(QStringList *topicList)
{
//    Image dealer class here
    if(targetDir.isEmpty())
        on_select_tardir_btn_clicked();
    ImageDataDealer *imgdealer = DataDealerFactory<ImageDataDealer>::getInstance()->createDataDealer();
    for(auto it = topicList->cbegin(); it != topicList->cend(); it++)
    {
        imgdealer->process(bagfileName, targetDir, *it);
    }
}

void MainWindow::deal_pose_data(QStringList *topicsList)
{
    if((*topicsList).contains("/tf"))
    {
        if(bag.isOpen() == false)
            bag.open(bagfileName.toStdString(), rosbag::bagmode::Read);
        ui->next_btn->setEnabled(true);
        tfWidget = new QListWidget;
        //		tfWidget->setHeader("Select two transformation frames");
        ui->tabWidget->addTab(tfWidget, "tf selector");
        ui->tabWidget->insertTab(1, tfWidget, "tf selector");
        ui->tabWidget->setCurrentIndex(1);
        std::vector<std::string> tps;
        for(auto it = (*topicsList).cbegin(); it != (*topicsList).cend(); ++it)
        {
            tps.push_back(it->toStdString());
        }
        rosbag::View view(bag, rosbag::TopicQuery(tps));
        BOOST_FOREACH(rosbag::MessageInstance const m, view)
        {
            tf2_msgs::TFMessageConstPtr tfStamped = m.instantiate<tf2_msgs::TFMessage>();
            for(int cnt = 0; cnt < tfStamped->transforms.size(); cnt++)
            {
                QString father_frame = QString::fromStdString(tfStamped->transforms.at(cnt).header.frame_id);
                QString child_frame = QString::fromStdString(tfStamped->transforms.at(cnt).child_frame_id);
                std::pair<QString, QString> tmpTF({father_frame, child_frame});
                if(std::find(tfVec.cbegin(), tfVec.cend(), tmpTF) == tfVec.cend())
                    tfVec.push_back(tmpTF);
            }
        }

        for(auto it = tfVec.cbegin(); it != tfVec.cend(); it++)
        {
            QString itemStr = it->first + QString("--->") + it->second;
            QListWidgetItem *item = new QListWidgetItem(itemStr, tfWidget);
            item->setCheckState(Qt::Unchecked);
        }


        ui->next_btn->setEnabled(true);
    }
}

void MainWindow::deal_pointCloud_data(QStringList *topicList)
{
    if(targetDir.isEmpty())
        on_select_tardir_btn_clicked();
    PointCloudDataDealer *pointCloudDataDealer =
            DataDealerFactory<PointCloudDataDealer>::getInstance()->createDataDealer() ;
    for(auto it = topicList->cbegin(); it != topicList->cend(); ++it)
    {
        pointCloudDataDealer->process(bagfileName, targetDir, *it);
    }
}



void MainWindow::setTopicsSelector()
{
    ui->treeWidget->setColumnCount(1);
    ui->treeWidget->setHeaderLabel("Select topics");
    ui->treeWidget->setHeaderHidden(false);
    QTreeWidgetItem *imgItems, *pcItems, *poseItems;
    QList<QTreeWidgetItem*> imgTopicItems;
    QList<QTreeWidgetItem*> pcTopicItems;
    QList<QTreeWidgetItem*> poseTopicItems;
    for(auto it = imageTopics.cbegin(); it != imageTopics.cend(); it++)
    {
        QTreeWidgetItem *item = new QTreeWidgetItem;
        item->setText(0, *it);
        item->setCheckState(0, Qt::Unchecked);
        imgTopicItems << item;
    }
    for(auto it = pointCloudTopics.cbegin(); it != pointCloudTopics.cend(); it++)
    {
        QTreeWidgetItem *item = new QTreeWidgetItem;
        item->setText(0, *it);
        item->setCheckState(0, Qt::Unchecked);
        pcTopicItems << item;
    }
    for(auto it = poseTopics.cbegin(); it != poseTopics.cend(); it++)
    {
        QTreeWidgetItem *item = new QTreeWidgetItem;
        item->setText(0, *it);
        item->setCheckState(0, Qt::Unchecked);
        poseTopicItems << item;
    }
    imgItems = new QTreeWidgetItem;
    pcItems = new QTreeWidgetItem;
    poseItems = new QTreeWidgetItem;
    imgItems->setText(0, "Image topics");
    pcItems->setText(0, "PointCloud topics");
    poseItems->setText(0, "Pose topics");
    QList<QTreeWidgetItem*> tpItems;
    tpItems << imgItems << pcItems << poseItems;
    imgItems->addChildren(imgTopicItems);
    pcItems->addChildren(pcTopicItems);
    poseItems->addChildren(poseTopicItems);
    ui->treeWidget->addTopLevelItems(tpItems);
    ui->treeWidget->expandAll();
    ui->treeWidget->show();
}

bool MainWindow::treeWidgetHasChecked()
{
    QTreeWidgetItemIterator it(ui->treeWidget);
    while(*it)
    {
        Qt::CheckState cs = (*it)->checkState(0);
        if(cs == Qt::Checked)
            return true;
        ++it;
    }
    return false;
}

QList<QStringList *> MainWindow::getCheckedTopics()
{
    QList<QStringList *> cpList;
    QStringList *imgList = new QStringList;
    QStringList *pcList = new QStringList;
    QStringList *poseList = new QStringList;
    cpList << imgList << pcList << poseList;
    for(int cnt = 0; cnt < ui->treeWidget->topLevelItemCount(); cnt++)
    {
        auto typeItem = ui->treeWidget->topLevelItem(cnt);
        for(int topicCnt = 0; topicCnt < typeItem->childCount(); topicCnt++)
        {
            auto item = typeItem->child(topicCnt);
            auto data = item->data(0, Qt::DisplayRole);
            if(item->checkState(0))
            {
                if(data.canConvert(QVariant::String))
                {
                    *(cpList.at(cnt)) << data.toString();
                }
            }
        }
    }
    return cpList;
}

void MainWindow::on_back_btn_clicked()
{
    if(ui->tabWidget->count() == 3)
        ui->tabWidget->removeTab(1);
    ui->tabWidget->setCurrentIndex(0);
    ui->next_btn->setEnabled(true);
    checkedTopics.empty();
    step1 = true;
}

void MainWindow::on_actionnext_triggered()
{
    on_next_btn_clicked();
}

void MainWindow::on_actionBack_triggered()
{
    on_back_btn_clicked();
}


void MainWindow::on_select_tardir_btn_clicked()
{
    QFileDialog *fd = new QFileDialog(this);
    fd->setFileMode(QFileDialog::Directory);
    fd->exec();
    targetDir = fd->selectedFiles().at(0);
}
